#include<Algorithm/intersection_curve_of_two_surfaces.h>
#include<tool/libcell_tools_octree.h>
#include<tool/libcell_octree_iterator.h>
#include<Mesh/libcell_iterator.h>
#define DEFAULT_THRESHOLD  1e-7 
#define quote lib_cell_quote

void mesh2_crossover_point_free(Mesh2_Crossover_Point*mcp)
{
    int_rb_tree_free(mcp->v2p);
    for(auto it=mcp->f2p->begin(mcp->f2p);it.it!=NULL;it++)
    {
        free_node((Node*)(it.second));
    }
    int_rb_tree_free(mcp->f2p);
    for(auto it=mcp->c2p->begin(mcp->c2p);it.it!=NULL;it++)
    {
        free_node((Node*)(it.second));
    }
    int_rb_tree_free(mcp->c2p);
   
    mcp->v2p=NULL;mcp->f2p=NULL;mcp->c2p=NULL;
} 
static inline int is_same_point(double *p1,double *p2)
{
    if(fabs(p1[0]-p2[0])<1e-6&&fabs(p1[1]-p2[1])<1e-6&&fabs(p1[2]-p2[2])<1e-6)
    {
        return 1;
    }
    return 0; 
}
static inline int is_same_point1(double *p1,double *p2)
{
    if(fabs(p1[0]-p2[0])<1e-5&&fabs(p1[1]-p2[1])<1e-5&&fabs(p1[2]-p2[2])<1e-5)
    {
        return 1;
    }
    return 0; 
}

static double** get_intersection_points_from_two_cells(Mesh* m,template_c* c1,template_c* c2)
{
    int len1=c1->vertices_size,len2=c2->vertices_size;
    double**poly1=(double**)malloc(sizeof(double*)*len1);
    double**poly2=(double**)malloc(sizeof(double*)*len2);
   
    Node* hfs=Mesh_adjust_halffaces(m,c1);
    int i=0;
    for(Node*nit=hfs;nit!=NULL;nit=(Node*)(nit->Next))
    {
        poly1[i]=((template_hf*)(nit->value))->vertices[0]->point;
        i++;
    }
    free_node(hfs);
    hfs=Mesh_adjust_halffaces(m,c2);
    i=0;
    for(Node*nit=hfs;nit!=NULL;nit=(Node*)(nit->Next))
    {
        poly2[i]=((template_hf*)(nit->value))->vertices[0]->point;
        i++;
    }
    free_node(hfs);
    // printf("intersection c1 ：%d c2:%d\n",c1->id,c2->id);

    double **re=get_intersection_points_from_two_polygons(poly1,len1,poly2,len2);
    // if(re==NULL)
    // {
    //     printf("intersection c1 ：%d c2:%d\n",c1->id,c2->id);
    // }
    free(poly1);free(poly2);  
    
    return re;
}



// mcp 储存了c元素上附着点的信息

static template_v* is_exist_same_point_in_cell(template_c* c,double* p,Mesh2_Crossover_Point* mcp)
{
    for(int i=0;i<c->vertices_size;i++)
    {
        template_v * v= (template_v*)(mcp->v2p->find(mcp->v2p,c->vertices[i]->id));
        if(v!=NULL&&is_same_point(p,v->point))
        {
            return v;
        }
    }
    for(Node* hfit=c->halffaces;hfit!=NULL;hfit=(Node*)(hfit->Next))
    {
        template_f* f=((template_hf*)(hfit->value))->face;
        Node* value=(Node*)(mcp->f2p->find(mcp->f2p,f->id));
        for(Node* nit=value;nit!=NULL;nit=(Node*)(nit->Next))
        {
           template_v*v=(template_v*)(nit->value);
           if(is_same_point(p,v->point))
           {
                return v;  
           }
        }
    }
    Node* value=(Node*)(mcp->c2p->find(mcp->c2p,c->id));
    for(Node* nit=value;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v*v=(template_v*)(nit->value);
        if(is_same_point(p,v->point))
        {
            return v;  
        }
    }
    return NULL;
}


// compute cosine value
static inline double cos_distance_point_to_line_segments1(double* p,double* p1,double* p2)
{
    double n[3]={p2[0]-p1[0],p2[1]-p1[1],p2[2]-p1[2]};
    double a[3]={p[0]-p1[0],p[1]-p1[1],p[2]-p1[2]}; 
    if(!normalize(n,3)||!normalize(a,3))
    {
        printf("normalize cuowu\n");
        return 0;
    }   
    double dis1=n[0]*a[0]+n[1]*a[1]+n[2]*a[2];
    return dis1; 
}

// mcp 储存c元素上附着点的信息
// scp是上面的逆信息
//
static void vertex_attachment_to_celln(template_c*c,template_v* nv,Mesh2_Crossover_Point* mcp,
    SCrossover_Point * scp)
{
    for(int i=0;i<c->vertices_size;i++)
    {
        // if(len==1)
        // {
        //     if(is_same_point1(nv->point,c->vertices[i]->point))
        //     {
        //         mcp->v2p->insert(mcp->v2p,c->vertices[i]->id,nv);
        //         scp->v=c->vertices[i];
        //         return ;
        //     }
        // }
        // else
        {
            if(is_same_point(nv->point,c->vertices[i]->point))
            {
                mcp->v2p->insert(mcp->v2p,c->vertices[i]->id,nv);
                scp->v=c->vertices[i];
                return ;
            }
        }
        
    } 
    for(Node* hfit=c->halffaces;hfit!=NULL;hfit=(Node*)(hfit->Next))
    {
        template_f* f=((template_hf*)(hfit->value))->face;
        double * l=get_line_equation_from_two_points(f->vertices[0]->point,f->vertices[1]->point);
        if(l==NULL)
        {
            continue;
        } 
        double distance= distance_from_point_to_line(nv->point,l);

        free(l);
        if(distance<1e-6)
        {
            Node* value=(Node*)(mcp->f2p->find(mcp->f2p,f->id));
            value=node_overlying(value,nv);
            mcp->f2p->insert(mcp->f2p,f->id,value); 
            scp->f=f;
            return;
        } 
    }
    Node* value=(Node*)(mcp->c2p->find(mcp->c2p,c->id));
    value=node_overlying(value, nv);
    mcp->c2p->insert(mcp->c2p,c->id, value); 
    scp->c=c; 
    return ;
}

static int is_two_cell_maybe_inter(template_c* c1,template_c* c2)
{
    double min1[3]={c1->vertices[0]->point[0],c1->vertices[0]->point[1],c1->vertices[0]->point[2] },
        max1[3]={c1->vertices[0]->point[0],c1->vertices[0]->point[1],c1->vertices[0]->point[2]};


    for(int i=1;i<c1->vertices_size;i++)
    {
        for(int j=0;j<3;j++)
        {
            c1->vertices[i]->point[j]<min1[j]?(min1[j]=c1->vertices[i]->point[j]):(c1->vertices[i]->point[j]>max1[j]?(max1[j]=c1->vertices[i]->point[j]):i=i);

        }
    }
    double min2[3]={c2->vertices[0]->point[0],c2->vertices[0]->point[1],c2->vertices[0]->point[2] },
        max2[3]={c2->vertices[0]->point[0],c2->vertices[0]->point[1],c2->vertices[0]->point[2]};
    
    for(int i=1;i<c2->vertices_size;i++)
    {
        for(int j=0;j<3;j++)
        {
            c2->vertices[i]->point[j]<min2[j]?(min2[j]=c2->vertices[i]->point[j]):(c2->vertices[i]->point[j]>max2[j]?(max2[j]=c2->vertices[i]->point[j]):i=i);

        }
    }

    if(min2[0]>max1[0]+DEFAULT_THRESHOLD||min2[1]>max1[1]+DEFAULT_THRESHOLD||min2[2]>max1[2]+DEFAULT_THRESHOLD)
    {
        return 0;
    }
    if(max2[0]<min1[0]+DEFAULT_THRESHOLD||max2[1]<min1[1]+DEFAULT_THRESHOLD||max2[2]<min1[2]+DEFAULT_THRESHOLD) 
    {
        return 0;
    }
    return 1;
}
static int is_two_cell_parallel(template_c* c1,template_c*c2)
{
    

}
//m是新建点的信息网格
//mcp1储存c1上元素的附着点信息
//mcp2储存c2上元素的附着点信息

static template_v** get_intersection_vertices_from_two_cells(template_c* c1,template_c* c2,Mesh2_Crossover_Point * mcp1,Mesh2_Crossover_Point * mcp2,Mesh* m)
{
    if(is_two_cell_maybe_inter(c1,c2)==0)
    {
        return NULL;
    }
    double ** points=get_intersection_points_from_two_cells(m,c1,c2);
    if(points==NULL)
    {return NULL;}
    int len=1;
    is_same_point(points[0],points[1])?(len=1):(len=2);

    // if(c1->id==7258||c1->id==7255)
    // {
    //     printf("%d len:%d\n",c1->id,len);
    // }

    template_v** re=(template_v**)malloc(sizeof(template_v*)*2);
    memset(re,0,sizeof(template_v*)*2);
    for(int i=0;i<len;i++)
    {
        re[i]=is_exist_same_point_in_cell(c1,points[i],mcp1);
        

        if(re[i]==NULL)
        { 
            // if(c1->id==7258||c1->id==7255)
            // {
            //     printf("is null\n");
            // }
            re[i]=m->create_vertexv(m,points[i],3);
            Crossover_Point *cp=(Crossover_Point*)malloc(sizeof(Crossover_Point));
            crossover_point_init(cp);  
            re[i]->prop=cp;
            vertex_attachment_to_celln(c1,re[i],mcp1,&(cp->scp[0]));
            vertex_attachment_to_celln(c2,re[i],mcp2,&(cp->scp[1]));
        } 
        // if(c1->id==7258||c1->id==7255)
        // {
        //     printf("null %d\n",re[i]->id);
        // }
    } 
    for(int i=0;i<2;i++)
    {
        SAFE_FREE(points[i]);
    } 
    free(points); 
    return re;
}


void  get_intersection_lines_of_two_nodes(Mesh* curve,Node* n1,Node* n2,Mesh2_Crossover_Point* mcp1,Mesh2_Crossover_Point* mcp2)
{ 
    if(n1==NULL||n2==NULL)
    {return ;}
   
    for(Node* it1=n1;it1!=NULL;it1=(Node*)(it1->Next))
    {
        template_c* c1=(template_c*)(it1->value);
        for(Node* it2=n2;it2!=NULL;it2=(Node*)(it2->Next))
        {
            template_c*c2=(template_c*)(it2->value);
            template_v** vs=get_intersection_vertices_from_two_cells(c1,c2,mcp1,mcp2,curve);
            if(vs==NULL)
            {continue;} 
            if(vs[0]!=NULL&&vs[1]!=NULL)
            {
                curve->create_facev(curve,vs,2); 
            } 
            free(vs);
        }
    } 
}
Mesh* get_intersection_lines_of_two_nodesn(Node* n1,Node* n2,Mesh2_Crossover_Point*mcp1,Mesh2_Crossover_Point*mcp2)
{
    Node* in=get_maybe_intersecting_areas_from_nodes(n1,n2);
    Mesh* re=(Mesh*)malloc(sizeof(Mesh));
    Mesh_init(re);
    re->simplex=1;
    re->manifold_require=0;

    for(Node* nit=in;nit!=NULL;nit=(Node*)(nit->Next))
    {

        Maybe_Intersecting_Areas* mia=(Maybe_Intersecting_Areas*)(nit->value);

        get_intersection_lines_of_two_nodes(re,mia->node1,mia->node2,mcp1,mcp2);
        maybe_intersecting_areas_free(mia);  

    }
    free_node(in);
    return re;
}
Mesh*  get_intersection_lines_of_two_meshs(Mesh* m1,Mesh* m2,Mesh2_Crossover_Point* mcp1,Mesh2_Crossover_Point*mcp2)
{ 
    Node* n1=NULL,*n2=NULL; 
    for(auto it=m1->c_begin(m1);it!=m1->c_end(m1);it++)
    {
        n1=node_overlying(n1,quote(it));
    } 
    for(auto it=m2->c_begin(m2);it!=m2->c_end(m2);it++)
    {
        n2=node_overlying(n2,quote(it));
    }
    Mesh* re=get_intersection_lines_of_two_nodesn(n1,n2,mcp1,mcp2);
    free_node(n1);free_node(n2);
   

    return re;
 }


static void merge_two_vertices(Mesh* m,template_v* v,template_v* v1,Mesh2_Crossover_Point* mcp1,Mesh2_Crossover_Point*mcp2)
{ 
    Crossover_Point* cp=(Crossover_Point*)(v1->prop),*cp1=(Crossover_Point*)(v->prop);
    Mesh2_Crossover_Point* mcps[2]={mcp1,mcp2};
    printf("erase v id:%d save id:%d\n",v1->id,v->id);
    for(int i=0;i<2;i++)
    {
        if(cp->scp[i].v!=NULL)
        { 
            mcps[i]->v2p->erase(mcps[i]->v2p,cp->scp[i].v->id);
        }
        else if(cp->scp[i].f!=NULL)
        { 
            Node*node=(Node*)mcps[i]->f2p->find(mcps[i]->f2p, cp->scp[i].f->id);
            node=node_delete_value(node,v1);
            if(node==NULL)
            {
                mcps[i]->f2p->erase(mcps[i]->f2p,cp->scp[i].f->id);
            }
            else
            {
                mcps[i]->f2p->insert(mcps[i]->f2p,cp->scp[i].f->id,node);
            }
        }
        else
        { 
            Node* node=(Node*)mcps[i]->f2p->find(mcps[i]->c2p,cp->scp[i].c->id); 
            node=node_delete_value(node,v1);
            if(node==NULL)
            {
                mcps[i]->c2p->erase(mcps[i]->c2p,cp->scp[i].c->id);
            }
            else
            {
                mcps[i]->c2p->insert(mcps[i]->c2p,cp->scp[i].c->id,node);
            }
        }
    }
    template_v* vs[2]={v,NULL};
    if(v1->faces!=NULL)
    {
        template_f* f=(template_f*)(v1->faces->value);

        f->vertices[0]==v1?(vs[1]=f->vertices[1]):(vs[1]=f->vertices[0]);
        printf("build id:%d\n",vs[1]->id);
        m->delete_face(m,*f,true);
        m->create_facev(m,vs,2); 
    }
    m->delete_vertex(m,*v1,true);
    
}

static inline int equal_topology(template_v* v1,template_v*v2)
{
    Crossover_Point* cp=(Crossover_Point*)(v1->prop),*cp1=(Crossover_Point*)(v2->prop);

    if(cp->scp[0].f==cp1->scp[0].f&&cp->scp[0].c==cp1->scp[0].c&&cp->scp[0].v==cp1->scp[0].v 
        &&cp->scp[1].f==cp1->scp[1].f&&cp->scp[1].c==cp1->scp[1].c&&cp->scp[1].v==cp1->scp[1].v 
        )
    {
        return 1;
    }

    return 0;
}
//
static  Node* adjust_edges_to_loop_from_nodes(Node* node,Mesh*m,Mesh2_Crossover_Point* mcp1,Mesh2_Crossover_Point*mcp2)
{
    if(node==NULL)
    {
        return NULL;
    }
    Node* n1=NULL,*n2=NULL;
    Node* nit=(Node*)(node->Next);n1=node;
    template_v* v=(template_v*)(n1->value),*v1=NULL;
    int flag=0;
    printf("v:%d %lf %lf %lf\n",v->id,v->point[0],v->point[1],v->point[2]);
    for(;nit!=NULL;nit=(Node*)(nit->Next))
    {

        v1=(template_v*)(nit->value);
        printf("v1:%d %lf %lf %lf\n",v1->id,v1->point[0],v1->point[1],v1->point[2]);

        if(equal_topology(v,v1))
        {
            printf("is equal\n");
            n2=nit;
            flag=1;
            break;
        }


        if(fabs(v->point[0]-v1->point[0])<1e-5&&fabs(v->point[1]-v1->point[1])<1e-5&&fabs(v->point[2]-v1->point[2])<1e-5)
        {
            printf("not equal\n");
            template_v*vs[2]={v,v1};
            m->create_facev(m,vs,2); 
            n2=nit;
            flag=2;
            break;
        }
    }
    if(n1!=NULL&&n2!=NULL&&flag==1)
    {
        printf("merge two vertices\n");
        merge_two_vertices(m,v,v1,mcp1,mcp2); 
    }
    //printf("node size:%d\n",node_size(node));

    Node*re=node_remove(node,n1);
    //printf("node size:%d\n",node_size(re));
    re=node_remove(re,n2);
    //printf("node size:%d\n",node_size(re));

    return re;
}

static int vertex_is_on_the_boundary_of_surfaces(Mesh* m, template_v*v)
{
    Crossover_Point*cp=(Crossover_Point*)(v->prop);
    for(int i=0;i<2;i++)
    {
        if(cp->scp[i].v!=NULL&&m->vertex_is_boundary(m,*(cp->scp[i].v)))
        {                   
            return 1;
        }
        else if(cp->scp[i].f!=NULL&&m->face_is_boundary(m,*(cp->scp[i].f)))
        {
            return 1;
        }
    } 
    return 0;
}




static int   is_face_on_face_and_adjust(Mesh* curve,template_f* f,Mesh2_Crossover_Point*mcp1,Mesh2_Crossover_Point*mcp2 )
{
    template_v* v1=f->vertices[0];
    template_v* v2=f->vertices[1];
    Crossover_Point*cp1=(Crossover_Point*)(v1->prop);
    Crossover_Point*cp2=(Crossover_Point*)(v2->prop);
    Mesh2_Crossover_Point* mcps[2]={mcp1,mcp2};

    template_v*vs[2]={0};
    int flag=0;
    for(int i=0;i<2;i++)
    {
        if(cp1->scp[i].f!=NULL&&cp2->scp[i].f==cp1->scp[i].f )
        {   
            flag=1;
            break;
        }
    }
    if(flag)
    {
        curve->delete_face(curve,*f,true);
        merge_two_vertices(curve,v2,v1,mcp1,mcp2);
    }
 
   return flag;
}

//检测并调整拓扑

void adjust_mesh_topology_of_cintersecting_curve(Mesh* m,Mesh2_Crossover_Point*mcp1,Mesh2_Crossover_Point*mcp2)
{
    //这种情况也允许存在，理论上不需要调整，需要调整的是剖分算法
    // int flag=1;
    // while(flag==1)
    // {
    //     flag=0;
    //     for(auto fit=m->f_begin(m);fit!=m->f_end(m);fit++)
    //     {
    //         flag=is_face_on_face_and_adjust(m,quote(fit),mcp1,mcp2 );
    //         if(flag)
    //         {
    //             break;
    //         }
    //     }
    // }
    

    // for(Node* nit=temp;nit!=NULL;nit=(Node*)(nit->Next) )
    // {
    //     curve->delete_face(curve,*f,true);

    //     merge_two_vertices(curve,v2,v1,mcp1,mcp2);
    // }


    Node* node1=NULL,*node4=NULL;
    printf("ajust mesh topology\n");
    for(auto it=m->v_begin(m);it!=m->v_end(m);it++)
    {
        int size=node_size(quote(it)->faces);
        if(size%2==1)
        {
            if(!vertex_is_on_the_boundary_of_surfaces(m,quote(it)))
            {
                node1=node_overlying(node1,quote(it));
            }
        }
        else if(size==4)
        {
            printf("jiance 4:%d\n",quote(it)->id );
            node4=node_overlying(node4,quote(it));
        }
    }   
    while(node1!=NULL)
    {
        printf("ajust once %d \n",node_size(node1));

        node1 = adjust_edges_to_loop_from_nodes(node1,m,mcp1,mcp2);

    } 
    free_node(node1);
    free_node(node4); 
}